#include <iostream>
 
#include "Timer/timer.01.h"
#include "GadgetSnapshotIO/gsio.05.h"
#include "Matrix/matrix.01.h"
#include "PotMinFinder/pmf.01.h"
#include "FoFReader/fofr.01.h"
#include "Toomre/toomre.02.h"

#define  GRAVITY     6.672e-8
#define  SOLAR_MASS  1.989e33
#define  CM_PER_MPC  3.085678e24

const bool COSFOF = true;
const bool NONCOS = false;

using namespace std;

struct Particle { 	
	dVector R;
	dVector V;
	double m; 
};

void ss_ToomreQ(const char *ifname, const char *opath, double r1, double r2, uint n, bool cosfofF=false) {
	
	
	double UnitMass_in_g = 1e10 * SOLAR_MASS;		// 10^10 solar masses 
	double UnitLength_in_cm = CM_PER_MPC / 1000;	// 1 kpc 
	double UnitVelocity_in_cm_per_s = 1e5;			// 1 km/sec 

	double UnitTime_in_s = UnitLength_in_cm / UnitVelocity_in_cm_per_s;
	double G = GRAVITY / pow(UnitLength_in_cm, 3) * UnitMass_in_g * pow(UnitTime_in_s, 2);
	//printf("G=%g\n", G);

	GSIO gsio;
	gsio.read(ifname);
	//gsio.print();
	
	FND fnd;
	fnd.decompose(ifname);
	
	
	vector<Particle> p, pD; Particle pi;
	
	if (cosfofF) {
		uint gid = 0;
		FoFR fofr;
		fofr.read(fnd.path.c_str(), fnd.ifileID);
		fofr.print();
		
		vector<uint> I(gsio.N[1]+gsio.N[2]+gsio.N[3]);
		for (uint i=0; i<I.size(); i++) I[gsio.Id[i]]=i;
	
		double a = gsio.Time;
		double u = 1000;
		
		for (uint i=0;i<fofr.grp[gid].Len; i++) {
			pi.R = *(Vector<float>*)(gsio.p[1][I[fofr.grp[gid].ids[i]]].Pos)*u*a;
			pi.m = gsio.p[1][I[fofr.grp[gid].ids[i]]].Mass;
			p.push_back(pi);
		}
		
		for (uint i=0;i<gsio.N[4]+gsio.N[5]; i++) {
			pi.R = *(Vector<float>*)gsio.p[4][i].Pos*u*a;
			pi.m = gsio.p[4][i].Mass;
			p.push_back(pi);
		}	
		
		for (uint i=0;i<gsio.N[4]; i++) {
			pi.R = *(Vector<float>*)gsio.p[4][i].Pos*u*a;
			pi.V = *(Vector<float>*)gsio.p[4][i].Vel*sqrt(a);
			pi.m = gsio.p[4][i].Mass;
			pD.push_back(pi);
		}
		
	} else  {
		
		for (uint i=0;i<gsio.N; i++) {
			pi.R = *(Vector<float>*)gsio.p[0][i].Pos;
			pi.m = gsio.p[0][i].Mass;
			p.push_back(pi);
		}
		
		for (uint i=0;i<gsio.N[2]; i++) {
			pi.R = *(Vector<float>*)gsio.p[2][i].Pos;
			pi.V = *(Vector<float>*)gsio.p[2][i].Vel;
			pi.m = gsio.p[2][i].Mass;
			pD.push_back(pi);
		}
		
	}
	
	/*
	PMF pmf;
	pmf.find(4, p, SQR(0.001));
	pmf.print();
	dVector Rcm = pmf.PC;
	*/
	dVector Rcm = findCM(pD, &pD[0].R, &pD[0].m );
	dVector Vcm = findCM(pD, &pD[0].V, &pD[0].m );
	
	for (uint i=0;i<pD.size(); i++) { pD[i].R-=Rcm; pD[i].V-=Vcm; }
	for (uint i=0;i<p.size(); i++) p[i].R-=Rcm;
	
	
	Matrix I;
	I.calc(pD);
	I.calcEigenVectorsGSL();
	
	dVector W=I.A; // Major Axis
	
	W.print();
	
	char filename[256];
	sprintf(filename,"%s%s", opath, fnd.fileID.c_str());
	mkdir(opath, 0777); 
	
	FILE *fp = fopen(filename, "w");
		
	double dr = pow(10, log10(r2/r1)/n);
	double fdr = pow(10, 0.5*log10(r2/r1)/n) - 1;	
	
	
	
	for(double r=r1*pow(dr, int(0.00*n)); r<r1*pow(dr, int(1.00*n)); r*=dr) {
	
		double velDispR = getVelDispR(pD,r,r*fdr,dVector(),dVector(),W);
		double epiKappa2 = getEpiKappa2(p,r,r*fdr,0, r*0.01*1.78583);
		double surfDen = getSurfDen(pD,r,r*fdr,dVector(),W);
	
		double Q =	velDispR*sqrt(epiKappa2)/(3.36*G*surfDen);
	
		fprintf(fp, "%10.3f    %10.2f  %10.3g %10.3g    %10.3g\n", r, velDispR, epiKappa2, surfDen, Q);
		printf("%s: %g\n", filename, r);
	}
	
	fclose(fp);
	
}


int main() {

	
	double r1 = 0.05;
	double r1 = 20;
	uint n = 100;
	
	string idir = "../../runs/step.12.orientation_InertiaMoment/";
	string odir = idir + string("A/Q/");
	ss_ToomreQ((idir+string("A/ss/snap_041")).c_str(),odir.c_str(), r1, r2, n, COSFOF);
	ss_ToomreQ((idir+string("A/ss/snap_041")).c_str(),odir.c_str(), r1, r2, n, COSFOF);
	ss_ToomreQ((idir+string("A/ss/snap_041")).c_str(),odir.c_str(), r1, r2, n, COSFOF);
	ss_ToomreQ((idir+string("A/ss/snap_041")).c_str(),odir.c_str(), r1, r2, n, COSFOF);
	ss_ToomreQ((idir+string("A/ss/snap_041")).c_str(),odir.c_str(), r1, r2, n, COSFOF);
	
	
	

	return 0;
}

